/**
 * @ Author: luoqi
 * @ Create Time: 2025-01-07 22:54
 * @ Modified by: luoqi
 * @ Modified time: 2025-03-13 22:14
 * @ Description:
 */

#ifndef _FOCC_H
#define _FOCC_H

#include <stdint.h>
#include <stdbool.h>
#include "../controller/lpf_1st.h"
#include "../controller/pid.h"
#include "../modules/qfoc.h"
#include "../algo/statistics.h"
#include "../modules/qwave.h"

typedef enum {
    FOCC_MOD_IDLE = 0,      // foc idle mode
    FOCC_MOD_I,
    FOCC_MOD_V,
    FOCC_MOD_P,
} FoccMod;  // foc control mode

typedef struct {
    uint64_t now;
    uint64_t last;
} FoccTime;

typedef struct {
    FoccMod cmod;

    qfp_t bemf_a;
    qfp_t bemf_b;
    qfp_t bemf_c;

    PidObj pid_iqloop;
    PidObj pid_idloop;
    PidObj pid_vloop;
    PidObj pid_ploop;

    qfp_t position;
    qfp_t origin;
    qfp_t acc;

    qfp_t iamax;    // a phase max current
    qfp_t ibmax;    // b phase max current
    qfp_t icmax;    // c phase max current
    qfp_t iqmax;    // q axis max current
    qfp_t idmax;    // d axis max current
    qfp_t vmax;     // velocity max
    qfp_t cimax;    // continuous current max

    Lpf1stObj oiqlpf;   // iq loop low pass filter
    Lpf1stObj oidlpf;   // id loop low pass filter
    Lpf1stObj ovlpf;    // velocity loop low pass filter
    Lpf1stObj oplpf;    // position loop low pass filter

    qfp_t eilim;    // current loop error limit
    qfp_t evlim;    // velocity loop error limit
    qfp_t eplim;    // position loop error limit

    qfp_t vff_coeff;    // velocity feed forward coefficient
    qfp_t pff_coeff;    // position feed forward coefficient

    uint32_t iloop_us;      // iq and id loop period time
    uint32_t vloop_us;      // velocity loop period time
    uint32_t ploop_us;      // position loop period time

    uint32_t iloop_calc_us; // iq and id loop calculate time
    uint32_t vloop_calc_us; // velocity loop calculate time
    uint32_t ploop_calc_us; // position loop calculate time
    uint32_t bemf_calc_us;  // bemf calculate time

    RmsObj eiq_rms;   // q axis current error rms
    RmsObj eid_rms;   // d axis current error rms
    RmsObj ev_rms;    // velocity error rms
    RmsObj ep_rms;    // position error rms

    FoccTime time_iloop;
    FoccTime time_vloop;
    FoccTime time_ploop;
} FoccObj;

typedef struct {
    FoccObj focc1;
    FoccObj focc2;
} Focc;

int focc_init(void);

QFocError focc_iqd_loop(FoccObj *focc, const QFocObj *foc, QFocOutput *output);

QFocError focc_vloop(FoccObj *focc, const QFocObj *foc, QFocOutput *output);

QFocError focc_ploop(FoccObj *focc, const QFocObj *foc, QFocOutput *output);

int focc_controller_clr(FoccObj *focc);

int focc_time_clr(FoccObj *focc);

Focc *focc_get(void);

#endif
